#include "varpot.h"

void varpot_init()
{
	// VarPot address, write mode
	TWIC.MASTER.ADDR = 0x58;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));

	// Configure TCON
	TWIC.MASTER.DATA = 0x40;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));
	TWIC.MASTER.DATA = 0xEE;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));
	TWIC.MASTER.DATA = 0xA0;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));
	TWIC.MASTER.DATA = 0xEE;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));

	TWIC.MASTER.CTRLC = 3; // Stop bits
}

void set_ir_power(char dir, uint8_t power)
{
	// VarPot address, write mode
	TWIC.MASTER.ADDR = 0x58;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));

	// Write command
	switch (dir)
	{
		case 'n' :
			TWIC.MASTER.DATA = 0x70;
			break;
		case 'e' :
			TWIC.MASTER.DATA = 0x10;
			break;
		case 's' :
			TWIC.MASTER.DATA = 0x00;
			break;
		case 'w' :
			TWIC.MASTER.DATA = 0x60;
			break;
	}			
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));
	
	// Write data
	TWIC.MASTER.DATA = power;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));
	TWIC.MASTER.CTRLC = 3; // Stop bits
}

uint8_t get_ir_power(char dir)
{
	uint8_t valread1, valread2;

	// VarPot address, write mode
	TWIC.MASTER.ADDR = 0x58;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));

	// Read command
	switch (dir)
	{
		case 'n' :
			TWIC.MASTER.DATA = 0x7C;
			break;
		case 'e' :
			TWIC.MASTER.DATA = 0x1C;
			break;
		case 's' :
			TWIC.MASTER.DATA = 0x0C;
			break;
		case 'w' :
			TWIC.MASTER.DATA = 0x6C;
			break;
	}			
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));

	// VarPot address, read mode
	TWIC.MASTER.ADDR = 0x59;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));
	TWIC.MASTER.CTRLC &= ~TWI_MASTER_ACKACT_bm;

	// Read data
	valread1 = TWIC.MASTER.DATA;
//	TWIC.MASTER.CTRLC &= !TWI_MASTER_ACKACT_bm;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));
	TWIC.MASTER.CTRLC |= TWI_MASTER_ACKACT_bm;
//		TWIC.MASTER.CTRLB &= ~TWI_MASTER_SMEN_bm;
	valread2 = TWIC.MASTER.DATA;
//	TWIC.MASTER.CTRLC |= TWI_MASTER_ACKACT_bm;
	while (!(TWIC.MASTER.STATUS & TWI_MASTER_CLKHOLD_bm));
	TWIC.MASTER.CTRLC = 3; // Stop bits
	return valread2;
}

